/**
 * @file pid.c
 * @author zhouh (17596492750@163.com)
 * @brief
 * @version 0.1
 * @date 2025-05-27
 */

#include "pid.h"
#include "pressure.h"
#include "add_funcation.h"
#include "definition.h"
#include "separator.h"

pid_t pressure_pid;
extern pressure_sensor_t press_sensor;

void pid_init(void)
{
	pressure_pid.kp = 200.0f;
	pressure_pid.ki = 0.01f;
	pressure_pid.kd = 0.08f;

	pressure_pid.integral = 0;
	pressure_pid.differential = 0;
}

/**
 * @brief	计算 PID 输出
 * @param	NOne
 * @retval	目标转速
 */
unsigned short pressure_pid_calculate(float pressure)
{
	unsigned short speed_temp = foc_control.speed_filtered;				// PID 输出赋初值
	pressure_pid.last_error = pressure_pid.error;						// 写入上一次误差
	pressure_pid.error = pressure - TARGET_PRESSURE; // 写入最新误差
	pressure_pid.integral += pressure_pid.error;						// 积分计算
	
	if (pressure_pid.integral > 200)
	{
		pressure_pid.integral = 200;
	}
	pressure_pid.differential = (pressure_pid.error - pressure_pid.last_error) / 0.01; // 微分计算（△t = 10ms）

	/* 计算输压力环输出值 */
	speed_temp = pressure_pid.kp * pressure_pid.error 
	           + pressure_pid.ki * pressure_pid.integral 
	           + pressure_pid.kd * pressure_pid.differential;

	/* 输出限幅 */
	if (speed_temp > MAX_SPEED)
	{
		speed_temp = MAX_SPEED;
	}
	else if (speed_temp < MIN_SPEED)
	{
		speed_temp = MIN_SPEED;
	}

	return speed_temp;
}
